\hypertarget{classcalico_1_1Trajectory}{}\doxysection{calico\+::Trajectory Class Reference}
\label{classcalico_1_1Trajectory}\index{calico::Trajectory@{calico::Trajectory}}


{\ttfamily \#include $<$trajectory.\+h$>$}

\doxysubsection*{Public Member Functions}
\begin{DoxyCompactItemize}
\item 
absl\+::\+Status \mbox{\hyperlink{classcalico_1_1Trajectory_a68f4f44651441e3d77bb13cd5b07cce7}{Fit\+Spline}} (const absl\+::flat\+\_\+hash\+\_\+map$<$ double, \mbox{\hyperlink{classcalico_1_1Pose3}{Pose3d}} $>$ \&poses\+\_\+world\+\_\+sensorrig, double knot\+\_\+frequency=\mbox{\hyperlink{classcalico_1_1Trajectory_ab50b8748cad2ad68a9e3800a56a14aa8}{k\+Default\+Knot\+Frequency}}, int spline\+\_\+order=\mbox{\hyperlink{classcalico_1_1Trajectory_a24009abb70b9197145dfeb5e6f589b51}{k\+Default\+Spline\+Order}})
\item 
int \mbox{\hyperlink{classcalico_1_1Trajectory_a7b3008b06c6bb5e9480a3131ad8a3f13}{Add\+Parameters\+To\+Problem}} (ceres\+::\+Problem \&problem)
\item 
\mbox{\Hypertarget{classcalico_1_1Trajectory_abce44ba657a445b2a73988c4788f0240}\label{classcalico_1_1Trajectory_abce44ba657a445b2a73988c4788f0240}} 
const \mbox{\hyperlink{classcalico_1_1BSpline}{BSpline}}$<$ 6 $>$ \& \mbox{\hyperlink{classcalico_1_1Trajectory_abce44ba657a445b2a73988c4788f0240}{spline}} () const
\begin{DoxyCompactList}\small\item\em Accessors for the spline. \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classcalico_1_1Trajectory_a9d1f75af0c4250fbbdeac8da6dce36f3}\label{classcalico_1_1Trajectory_a9d1f75af0c4250fbbdeac8da6dce36f3}} 
\mbox{\hyperlink{classcalico_1_1BSpline}{BSpline}}$<$ 6 $>$ \& {\bfseries spline} ()
\item 
absl\+::\+Status\+Or$<$ std\+::vector$<$ \mbox{\hyperlink{classcalico_1_1Pose3}{Pose3d}} $>$ $>$ \mbox{\hyperlink{classcalico_1_1Trajectory_ac20a0fc8fb0aabd138d34dd2935c8cf2}{Interpolate}} (const std\+::vector$<$ double $>$ \&interp\+\_\+times) const
\item 
\mbox{\Hypertarget{classcalico_1_1Trajectory_a03de815880e8367a6c110dace3b20090}\label{classcalico_1_1Trajectory_a03de815880e8367a6c110dace3b20090}} 
const absl\+::flat\+\_\+hash\+\_\+map$<$ double, \mbox{\hyperlink{classcalico_1_1Pose3}{Pose3d}} $>$ \& \mbox{\hyperlink{classcalico_1_1Trajectory_a03de815880e8367a6c110dace3b20090}{trajectory}} () const
\begin{DoxyCompactList}\small\item\em Setter/getter for the internal map between timestamp and pose. \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classcalico_1_1Trajectory_a18b5725c67dc3eb84def878ab07dbcd4}\label{classcalico_1_1Trajectory_a18b5725c67dc3eb84def878ab07dbcd4}} 
absl\+::flat\+\_\+hash\+\_\+map$<$ double, \mbox{\hyperlink{classcalico_1_1Pose3}{Pose3d}} $>$ \& {\bfseries trajectory} ()
\item 
\mbox{\Hypertarget{classcalico_1_1Trajectory_ad402e9a34842a3cf98e944415868a7e5}\label{classcalico_1_1Trajectory_ad402e9a34842a3cf98e944415868a7e5}} 
\mbox{\hyperlink{structcalico_1_1TrajectoryEvaluationParams}{Trajectory\+Evaluation\+Params}} \mbox{\hyperlink{classcalico_1_1Trajectory_ad402e9a34842a3cf98e944415868a7e5}{Get\+Evaluation\+Params}} (double stamp) const
\begin{DoxyCompactList}\small\item\em Get the parameters needed to evaluate the spline for a given timestamp. \end{DoxyCompactList}\end{DoxyCompactItemize}
\doxysubsection*{Static Public Member Functions}
\begin{DoxyCompactItemize}
\item 
{\footnotesize template$<$typename T $>$ }\\static \mbox{\hyperlink{classcalico_1_1Pose3}{Pose3}}$<$ T $>$ \mbox{\hyperlink{classcalico_1_1Trajectory_a60aa4cc5ee41e985c83259612e036530}{Vector\+To\+Pose3}} (const Eigen\+::\+Vector$<$ T, 6 $>$ \&vector)
\end{DoxyCompactItemize}
\doxysubsection*{Static Public Attributes}
\begin{DoxyCompactItemize}
\item 
\mbox{\Hypertarget{classcalico_1_1Trajectory_a24009abb70b9197145dfeb5e6f589b51}\label{classcalico_1_1Trajectory_a24009abb70b9197145dfeb5e6f589b51}} 
static constexpr int \mbox{\hyperlink{classcalico_1_1Trajectory_a24009abb70b9197145dfeb5e6f589b51}{k\+Default\+Spline\+Order}} = 6
\begin{DoxyCompactList}\small\item\em Default spline order of 6. \end{DoxyCompactList}\item 
\mbox{\Hypertarget{classcalico_1_1Trajectory_ab50b8748cad2ad68a9e3800a56a14aa8}\label{classcalico_1_1Trajectory_ab50b8748cad2ad68a9e3800a56a14aa8}} 
static constexpr double \mbox{\hyperlink{classcalico_1_1Trajectory_ab50b8748cad2ad68a9e3800a56a14aa8}{k\+Default\+Knot\+Frequency}} = 10
\begin{DoxyCompactList}\small\item\em Default knot frequency at 10Hz. \end{DoxyCompactList}\end{DoxyCompactItemize}


\doxysubsection{Detailed Description}
\mbox{\hyperlink{classcalico_1_1Trajectory}{Trajectory}} class. Takes in discrete world-\/from-\/sensorrig poses and encodes them as a 6-\/DOF B-\/\+Spline. 

\doxysubsection{Member Function Documentation}
\mbox{\Hypertarget{classcalico_1_1Trajectory_a7b3008b06c6bb5e9480a3131ad8a3f13}\label{classcalico_1_1Trajectory_a7b3008b06c6bb5e9480a3131ad8a3f13}} 
\index{calico::Trajectory@{calico::Trajectory}!AddParametersToProblem@{AddParametersToProblem}}
\index{AddParametersToProblem@{AddParametersToProblem}!calico::Trajectory@{calico::Trajectory}}
\doxysubsubsection{\texorpdfstring{AddParametersToProblem()}{AddParametersToProblem()}}
{\footnotesize\ttfamily int calico\+::\+Trajectory\+::\+Add\+Parameters\+To\+Problem (\begin{DoxyParamCaption}\item[{ceres\+::\+Problem \&}]{problem }\end{DoxyParamCaption})}

Add internal parameters to a ceres problem. Any internal parameters set to constant are marked as such in the problem. Returns the total number of parameters added to the problem. \mbox{\Hypertarget{classcalico_1_1Trajectory_a68f4f44651441e3d77bb13cd5b07cce7}\label{classcalico_1_1Trajectory_a68f4f44651441e3d77bb13cd5b07cce7}} 
\index{calico::Trajectory@{calico::Trajectory}!FitSpline@{FitSpline}}
\index{FitSpline@{FitSpline}!calico::Trajectory@{calico::Trajectory}}
\doxysubsubsection{\texorpdfstring{FitSpline()}{FitSpline()}}
{\footnotesize\ttfamily absl\+::\+Status calico\+::\+Trajectory\+::\+Fit\+Spline (\begin{DoxyParamCaption}\item[{const absl\+::flat\+\_\+hash\+\_\+map$<$ double, \mbox{\hyperlink{classcalico_1_1Pose3}{Pose3d}} $>$ \&}]{poses\+\_\+world\+\_\+sensorrig,  }\item[{double}]{knot\+\_\+frequency = {\ttfamily \mbox{\hyperlink{classcalico_1_1Trajectory_ab50b8748cad2ad68a9e3800a56a14aa8}{k\+Default\+Knot\+Frequency}}},  }\item[{int}]{spline\+\_\+order = {\ttfamily \mbox{\hyperlink{classcalico_1_1Trajectory_a24009abb70b9197145dfeb5e6f589b51}{k\+Default\+Spline\+Order}}} }\end{DoxyParamCaption})}

Add discrete poses representing a time-\/parameterized trajectory. A new spline will be fit to these poses with a specified knot frequency and spline order. Returns {\ttfamily absl\+::\+Ok\+Status()} if successful, an error otherwise. ~\newline
~\newline
{\ttfamily poses\+\_\+world\+\_\+body} is a map between a {\ttfamily double} timestamp in seconds and {\ttfamily Pose3d} world-\/from-\/sensorrig 6-\/DOF pose\+: \[ \mathbf{T}^w_r(t) = \left[\begin{matrix}\mathbf{R}^w_r&\mathbf{t}^w_{wr}\\ \mathbf{0}&1\end{matrix}\right] \] {\ttfamily knot\+\_\+frequency} is the knot rate of the spline. If {\ttfamily poses\+\_\+world\+\_\+body} contains timestamps spanning from $t_0$ to $t_f$ seconds, this function will yield a spline with $n_{knots} = \left(t_f - t_0\right) \left(\text{knot_frequency}\right)$~\newline
~\newline
{\ttfamily spline\+\_\+order} is the order of the spline. A spline order of $n$ implies that the pose at any given time is\+: \[ \mathbf{T}^w_r(t) = \left[\begin{matrix}\boldsymbol{\Phi}^w_r(t)\\\ \mathbf{t}^w_{wr}(t)\end{matrix}\right] = \mathbf{c}_{i,0} + \mathbf{c}_{i,1}t + ... + \mathbf{c}_{i,n}t^n \] where $\mathbf{c}_{i,j}$ is the j\textquotesingle{}th control point for the i\textquotesingle{}th spline segment. \mbox{\Hypertarget{classcalico_1_1Trajectory_ac20a0fc8fb0aabd138d34dd2935c8cf2}\label{classcalico_1_1Trajectory_ac20a0fc8fb0aabd138d34dd2935c8cf2}} 
\index{calico::Trajectory@{calico::Trajectory}!Interpolate@{Interpolate}}
\index{Interpolate@{Interpolate}!calico::Trajectory@{calico::Trajectory}}
\doxysubsubsection{\texorpdfstring{Interpolate()}{Interpolate()}}
{\footnotesize\ttfamily absl\+::\+Status\+Or$<$ std\+::vector$<$ \mbox{\hyperlink{classcalico_1_1Pose3}{Pose3d}} $>$ $>$ calico\+::\+Trajectory\+::\+Interpolate (\begin{DoxyParamCaption}\item[{const std\+::vector$<$ double $>$ \&}]{interp\+\_\+times }\end{DoxyParamCaption}) const}

Interpolate the trajectory at given timestamps. Returns a vector of world-\/from-\/sensorrig poses evaluated in the order of the timestamps. Returns an error if something went wrong during interpolation. For example, attempting to interpolate on out-\/of-\/bounds timestmaps. ~\newline
~\newline
{\ttfamily interp\+\_\+times} is the timestamps to query the spline. No assumptions are made about order. \mbox{\Hypertarget{classcalico_1_1Trajectory_a60aa4cc5ee41e985c83259612e036530}\label{classcalico_1_1Trajectory_a60aa4cc5ee41e985c83259612e036530}} 
\index{calico::Trajectory@{calico::Trajectory}!VectorToPose3@{VectorToPose3}}
\index{VectorToPose3@{VectorToPose3}!calico::Trajectory@{calico::Trajectory}}
\doxysubsubsection{\texorpdfstring{VectorToPose3()}{VectorToPose3()}}
{\footnotesize\ttfamily template$<$typename T $>$ \\
static \mbox{\hyperlink{classcalico_1_1Pose3}{Pose3}}$<$T$>$ calico\+::\+Trajectory\+::\+Vector\+To\+Pose3 (\begin{DoxyParamCaption}\item[{const Eigen\+::\+Vector$<$ T, 6 $>$ \&}]{vector }\end{DoxyParamCaption})\hspace{0.3cm}{\ttfamily [inline]}, {\ttfamily [static]}}

Convert a 6-\/DOF vector to \mbox{\hyperlink{classcalico_1_1Pose3}{Pose3}} type. First three elements of the vector are expected to be an $SO(3)$ log map vector, and the last three are the position values.~\newline
~\newline
 \[ \text{vector}=\left[\begin{matrix}\boldsymbol{\Phi}\\\mathbf{t}\end{matrix}\right]\\ \boldsymbol{\Phi} = \left[\begin{matrix}\Phi_x\\\Phi_y\\\Phi_z\end{matrix}\right]\\ \mathbf{R} = \exp\left([\boldsymbol{\Phi}]_\times\right)\\ \left[\boldsymbol{\Phi}\right]_\times = \left[\begin{matrix}0 &-\Phi_z&\Phi_y\\ \Phi_z&0&-\Phi_x\\ -\Phi_y&\Phi_x&0\end{matrix}\right] \] 

The documentation for this class was generated from the following files\+:\begin{DoxyCompactItemize}
\item 
calico/trajectory.\+h\item 
calico/trajectory.\+cpp\end{DoxyCompactItemize}
